# encoding=utf-8
# pip install pyserial -i https://pypi.tuna.tsinghua.edu.cn/simple
import time

from DistanceTools import *
import serial
from serial.serialutil import PortNotOpenError


class STP23L:
    def __init__(self, com_name: str):
        # 串口节点, 波特率, 停止位, 奇偶校验位, 流控制
        # 230400, 8bits, 1, 无, 无
        # ser = serial.Serial("/dev/ttyACM0", 230400)
        self.com_name = com_name
        self.ser = serial.Serial(self.com_name, 230400)
        if not self.ser.isOpen():
            raise Exception("{}, not init success".format(self.ser))

        # 程序状态位
        self.state = 0  # 状态位
        self.crc = 0x00  # 校验和
        self.cnt = 0  # 一帧12个点的计数
        self.PACK_FLAG = 0  # 命令标志位
        self.data_len = 0  # 数据长度
        self.timestamp = 0  # 时间戳
        self.state_flag = 1  # 转入数据接收标志位, uint8

        self.state_num = 0
        self.receive_cnt = 0

        # 程序数据位
        self.Pack_Data = [LidarPointTypedef() for i in range(12)]
        self.Pack_sum = LidarPointTypedef()

        # 数据输出
        self.decode_time = time.perf_counter()
        self.decode_data = LidarPointTypedef()

        # 循环跳出
        self.close_flag = False

    def get_decode_data(self) -> (float, LidarPointTypedef):
        return self.decode_time, self.decode_data

    def receive_data(self):
        while not self.close_flag:
            try:
                # 串口信息读取
                temp_data = self.ser.read(1)

                # 命令解码
                self.uart_data_receive_command(temp_data)

                # 数据解码
                self.uart_data_receive_data(temp_data)
            except TypeError as e:
                if str(e) == 'an integer is required (got type NoneType)':
                    pass
            except PortNotOpenError as p_e:
                if not self.close_flag:
                    print("串口链接失败, 1s后重新链接")
                    time.sleep(1)
                    self.ser.close()
                    self.ser = serial.Serial(self.com_name, 230400)

    def uart_data_receive_data(self, byte_data: bytes):
        temp_data = int.from_bytes(byte_data, byteorder='little')
        if self.PACK_FLAG == PACK_GET_DISTANCE and self.state_flag == 0:
            if self.state > 9:
                if self.state < 190:
                    self.state_num = (self.state - 10) % 15
                    self.receive_package(temp_data)
                elif self.state == 190:
                    self.timestamp = temp_data
                    self.state += 1
                    self.crc = (self.crc + temp_data) % 256
                elif self.state == 191:
                    self.timestamp = (temp_data << 8) + self.timestamp
                    self.state += 1
                    self.crc = (self.crc + temp_data) % 256
                elif self.state == 192:
                    self.timestamp = (temp_data << 16) + self.timestamp
                    self.state += 1
                    self.crc = (self.crc + temp_data) % 256
                elif self.state == 193:
                    self.timestamp = (temp_data << 24) + self.timestamp
                    self.state += 1
                    self.crc = (self.crc + temp_data) % 256
                elif self.state == 194:
                    if self.crc == temp_data:
                        # 数据校验成功
                        self.receive_cnt += 1
                        self.decode_package()
                    self.crc = 0x00
                    self.state = 0
                    self.state_flag = 1
                    self.cnt = 0

    def decode_package(self):
        decode_count = 0
        Pack_sum = LidarPointTypedef()
        for i in range(12):
            if self.Pack_Data[i].distance != 0:
                decode_count += 1
                Pack_sum.distance += self.Pack_Data[i].distance
                Pack_sum.noise += self.Pack_Data[i].noise
                Pack_sum.peak += self.Pack_Data[i].peak
                Pack_sum.confidence += self.Pack_Data[i].confidence
                Pack_sum.intg += self.Pack_Data[i].intg
                Pack_sum.reftof += self.Pack_Data[i].reftof

        if decode_count != 0:
            self.decode_time = time.perf_counter()
            self.decode_data.distance = Pack_sum.distance / decode_count
            self.decode_data.noise = Pack_sum.noise / decode_count
            self.decode_data.peak = Pack_sum.peak / decode_count
            self.decode_data.confidence = Pack_sum.confidence / decode_count
            self.decode_data.intg = Pack_sum.intg / decode_count
            self.decode_data.reftof = Pack_sum.reftof / decode_count

    def receive_package(self, temp_data: int):
        if self.state_num == 0:
            # 距离数据低8位
            self.Pack_Data[self.cnt].distance = temp_data
            self.crc = (self.crc + temp_data) % 256
            self.state += 1
        elif self.state_num == 1:
            # 距离数据
            self.Pack_Data[self.cnt].distance = (temp_data << 8) + self.Pack_Data[self.cnt].distance
            self.crc = (self.crc + temp_data) % 256
            self.state += 1
        elif self.state_num == 2:
            # 环境噪音低八位
            self.Pack_Data[self.cnt].noise = temp_data
            self.crc = (self.crc + temp_data) % 256
            self.state += 1
        elif self.state_num == 3:
            self.Pack_Data[self.cnt].noise = (temp_data << 8) + self.Pack_Data[self.cnt].noise
            self.crc = (self.crc + temp_data) % 256
            self.state += 1
        elif self.state_num == 4:
            self.Pack_Data[self.cnt].peak = temp_data
            self.crc = (self.crc + temp_data) % 256
            self.state += 1
        elif self.state_num == 5:
            self.Pack_Data[self.cnt].peak = (temp_data << 8) + self.Pack_Data[self.cnt].peak
            self.crc = (self.crc + temp_data) % 256
            self.state += 1
        elif self.state_num == 6:
            self.Pack_Data[self.cnt].peak = (temp_data << 16) + self.Pack_Data[self.cnt].peak
            self.crc = (self.crc + temp_data) % 256
            self.state += 1
        elif self.state_num == 7:
            self.Pack_Data[self.cnt].peak = (temp_data << 24) + self.Pack_Data[self.cnt].peak
            self.crc = (self.crc + temp_data) % 256
            self.state += 1
        elif self.state_num == 8:
            self.Pack_Data[self.cnt].confidence = temp_data
            self.crc = (self.crc + temp_data) % 256
            self.state += 1
        elif self.state_num == 9:
            self.Pack_Data[self.cnt].intg = temp_data
            self.crc = (self.crc + temp_data) % 256
            self.state += 1
        elif self.state_num == 10:
            self.Pack_Data[self.cnt].intg = (temp_data << 8) + self.Pack_Data[self.cnt].intg
            self.crc = (self.crc + temp_data) % 256
            self.state += 1
        elif self.state_num == 11:
            self.Pack_Data[self.cnt].intg = (temp_data << 16) + self.Pack_Data[self.cnt].intg
            self.crc = (self.crc + temp_data) % 256
            self.state += 1
        elif self.state_num == 12:
            self.Pack_Data[self.cnt].intg = (temp_data << 24) + self.Pack_Data[self.cnt].intg
            self.crc = (self.crc + temp_data) % 256
            self.state += 1
        elif self.state_num == 13:
            self.Pack_Data[self.cnt].reftof = temp_data
            self.crc = (self.crc + temp_data) % 256
            self.state += 1
        elif self.state_num == 14:
            self.Pack_Data[self.cnt].reftof = (temp_data << 8) + self.Pack_Data[self.cnt].reftof
            self.crc = (self.crc + temp_data) % 256
            self.state += 1

    def uart_data_receive_command(self, byte_data: bytes):
        temp_data = int.from_bytes(byte_data, byteorder='little')
        if self.state < 4:
            if temp_data == HEADER:
                self.state += 1
            else:
                self.state = 0
        elif 3 < self.state < 10:
            if self.state == 4:
                if temp_data == device_address:
                    self.state += 1
                    self.crc = (self.crc + temp_data) % 256
                else:
                    self.state = 0
                    self.crc = 0x00
            elif self.state == 5:
                if temp_data == PACK_GET_DISTANCE:
                    self.PACK_FLAG = PACK_GET_DISTANCE
                    self.state += 1
                    self.crc = (self.crc + temp_data) % 256
                elif temp_data == PACK_RESET_SYSTEM:
                    self.PACK_FLAG = PACK_RESET_SYSTEM
                    self.state += 1
                    self.crc = (self.crc + temp_data) % 256
                elif temp_data == PACK_STOP:
                    self.PACK_FLAG = PACK_STOP
                    self.state += 1
                    self.crc = (self.crc + temp_data) % 256
                elif temp_data == PACK_ACK:
                    self.PACK_FLAG = PACK_ACK
                    self.state += 1
                    self.crc = (self.crc + temp_data) % 256
                elif temp_data == PACK_VERSION:
                    self.PACK_FLAG = PACK_VERSION
                    self.state += 1
                    self.crc = (self.crc + temp_data) % 256
                else:
                    self.state = 0
                    self.crc = 0x00
            elif self.state == 6:
                if temp_data == chunk_offset:
                    self.state += 1
                    self.crc = (self.crc + temp_data) % 256
                else:
                    self.state = 0
                    self.crc = 0x00
            elif self.state == 7:
                if temp_data == chunk_offset:
                    self.state += 1
                    self.crc = (self.crc + temp_data) % 256
                else:
                    self.state = 0
                    self.crc = 0x00
            elif self.state == 8:
                self.data_len = temp_data
                self.state += 1
                self.crc = (self.crc + temp_data) % 256
            elif self.state == 9:
                self.data_len = self.data_len + (temp_data << 8)
                self.state += 1
                self.crc = (self.crc + temp_data) % 256
        elif self.state == 10:
            self.state_flag = 0

    def close(self):
        self.close_flag = True
        self.ser.close()


if __name__ == '__main__':
    import threading

    # stp = STP23L("/dev/ttyACM0")
    stp = STP23L("COM3")

    t = threading.Thread(target=stp.receive_data)
    t.start()

    start = time.perf_counter()
    while True:
        end = time.perf_counter()
        if end - start > 30:
            break

        time.sleep(0.1)

        data_t, data_d = stp.get_decode_data()
        if time.perf_counter() - data_t > 1:
            print("距离上次时间太久，此数据舍弃")
        else:
            print("---------------------------------")
            print("时间差: ", time.perf_counter() - data_t)
            print("测量距离: ", data_d.distance, " mm")
            print("测量噪音: ", data_d.noise)
            print("测量反光强度: ", data_d.peak)
            print("测量可信度: ", data_d.confidence)
            print("积分次数: ", data_d.intg)
            print("温度表征值: ", data_d.reftof)

    stp.close()
